/**
  ******************************************************************************
  * File Name          : rocr6_driver.cpp
  * Description       : rocr6_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <rocr6_driver/rocr6_driver.h>

std::string _port;
static int status = 0;
static double last_gripper = 0;

double rx_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double tx_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  接收rocr6_msgs/Goal话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void moveCB(const rocr6_msgs::Move::ConstPtr & msg)
{
    tx_data[0] = rocr6_tf_tdata(msg->j1);
    tx_data[1] = rocr6_tf_tdata(msg->j2);
    tx_data[2] = rocr6_tf_tdata(msg->j3);
    tx_data[3] = rocr6_tf_tdata(msg->j4);
    tx_data[4] = rocr6_tf_tdata(msg->j5);
    tx_data[5] = rocr6_tf_tdata(msg->j6);

    //控制器插补模式运行
    printf("joints:[% 0.4f % 0.4f % 0.4f % 0.4f % 0.4f %0.4f]\r\n", 
        tx_data[0]/10, tx_data[1]/10, tx_data[2]/10, tx_data[3]/10, tx_data[4]/10, tx_data[5]/10);
    rocr6_txSig(1, 0, tx_data);  
}

/**
  * @brief  接收rocr6_msgs/Gripper话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void gripperCB(const rocr6_msgs::Gripper::ConstPtr & msg)
{
    double gripper = msg->state;
    double txd[6]= {0, 0, 0, 0, 0, 0};
    if(gripper == last_gripper)
    {
        //夹爪状态相同，不用处理
    }
    last_gripper = gripper;
    txd[0] = gripper;
    rocr6_txSig(0x55, 0, txd); 
}

/**
  * @brief  机械臂驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "rocr6_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(100);   //100hz
    std_msgs::Bool motion_status;

    ros::Publisher pub_joints = nh.advertise<sensor_msgs::JointState>("/robot_driver/joint_states", 100);
    ros::Publisher pub_states = nh.advertise<std_msgs::Bool>("/robot_driver/robot_state", 100);

    ros::Subscriber sub_goal = nh.subscribe("/robot_driver/move", 100, moveCB);
    ros::Subscriber sub_gripper = nh.subscribe("/robot_driver/gripper", 100, gripperCB);

    sensor_msgs::JointState joint_state;

    nh.getParam("/rocr6_driver/joint_names", joint_state.name);
    nh.getParam("/rocr6_driver/joint_position", joint_state.position);
    nh.getParam("/rocr6_driver/port", _port);

    if(rocr6_init(_port.c_str()))
    {
        //开启机械臂抱闸，并延时以等待抱闸完全开启
        rocr6_brake(1);
        usleep(6000000);
        printf("[ROCR6]: open broadcast.\r\n");
        rocr6_broadcast(1);
    }
    else 
    {
        exit(1);
    }

    while (ros::ok())
    {
        //关节弧度反馈
        status = rocr6_rxSig(rx_data);
        joint_state.header.stamp = ros::Time::now();
        for(int i=0; i<JOINT_MAX; i++)
        {
          joint_state.position[i] =rocr6_tf_rdata(rx_data[i]);  
        }
        pub_joints.publish(joint_state);

        if(status==3)
        {
            motion_status.data = true;
        }
        else
        {
            motion_status.data = false;
        }
        pub_states.publish(motion_status);

        ros::spinOnce();
        loop_rate.sleep();
    }
     
    printf("[ROCR6]: close broadcast.\r\n");
    rocr6_broadcast(0);
    //关闭机械臂抱闸
    //rocr6_brake(0);
    rocr6_deinit();

    return 0;
}
